
package edu.moravian.CollisionPhysics;

import edu.moravian.entity.AbstractEntity;
import edu.moravian.math.Point2D;
import edu.moravian.math.Vector2D;
import edu.moravian.world.WorldDescriptor;

/**
 * This is a collision detection and response engine for AbstractEntities
 * 
 * This engine makes assumptions about the entities. Specifically we assume 
 * the entities are basically circles represented by their radius.
 * 
 * @author ronaghan
 */
public class EntityCollisionEngine
{
    private double elasticity;
    
    /**
     * Create a new physics engine based on the specified elasticity 
     * 
     * @param elasticity the worlds elasticity
     */
    public EntityCollisionEngine(double elasticity)
    {
        this.elasticity = elasticity;
    }

    
    /**
     * Get the engines elasticity 
     * 
     * @return the elasticity
     */
    public double getElasticity()
    {
        return elasticity;
    }


    /**
     * Set the engines elasticity
     * 
     * @param elasticity 
     */
    public void setElasticity(double elasticity)
    {
        this.elasticity = elasticity;
    }
    

    
    /**
     * Check if an entity will collide with the bounds of a rectangular 
     * world during the specified duration of time (in seconds)
     * 
     * @param entity the entity to check for collisions with the world
     * @param world the world descriptor for information about the world dimensions
     * @param delta the time frame to check for collisions during     
     * 
     * @return true if the entity will collide with the world bounds in delta time
     */
    public synchronized boolean willCollide(AbstractEntity entity,  WorldDescriptor world, double delta)
    {
        Vector2D heading = entity.getHeading();
        Point2D location = entity.getLocation();
        double radius = entity.getRadius();
        
        double xMin = world.getMinX() + radius;
        double xMax = world.getMaxX() - radius; 
        double yMin = world.getMinY() + radius;
        double yMax = world.getMaxY() - radius;
       
            
                
        if(location.getX() - radius <= xMin && heading.getX() < 0)
        {
            return true;
        }
        
        if (location.getX() + radius >= xMax && heading.getX() > 0)
        {
            return true;
        }
        
        if(location.getY() - radius <= yMin && heading.getY() < 0)
        {
            return true;
        }
        
        if (location.getY() + radius >= yMax && heading.getY() > 0)
        {
            return true;
        }

        return false;
    }


    
    /**
     * Respond to a collision between an entity and the world bounds.
     * This simply reflects the heading. 
     * 
     * @param entity the entity colliding with the world
     * @param world the world descriptor for information about the world dimensions
     */
    public synchronized void collisionResponse(AbstractEntity entity,  WorldDescriptor world)
    {
        Vector2D heading = entity.getHeading();
        Point2D location = entity.getLocation();
        double radius = entity.getRadius();
        
        double xMin = world.getMinX() + radius;
        double xMax = world.getMaxX() - radius; 
        double yMin = world.getMinY() + radius;
        double yMax = world.getMaxY() - radius;
                
        
        if(location.getX() <= xMin && heading.getX() < 0)
        {
            entity.getHeading().reflectX();
        }
        
        else if (location.getX() >= xMax && heading.getX() > 0)
        {
             entity.getHeading().reflectX();
        }
        
        
        if(location.getY() <= yMin && heading.getY() < 0)
        {
             entity.getHeading().reflectY();
        }
        
        else if (location.getY() >= yMax && heading.getY() > 0)
        {
             entity.getHeading().reflectY();
        }
    }           
    
    
    
    /**
     * Check if there will be a collision between two entities 
     * during the specified duration of time (in seconds).
     * 
     * @param One the first entity 
     * @param Two the second entity
     * @param delta the duration in which to check for a collision in seconds
     * 
     * @return whether or not entity one and two will collide in delta seconds
     */
    public synchronized boolean willCollide(AbstractEntity One, AbstractEntity Two, double delta)
    {
        Vector2D oneToTwo = Two.getLocation().minus(One.getLocation());
        Vector2D relativeVelocity = One.getVelocity().minus(Two.getVelocity());
        relativeVelocity.timesEquals(delta);
        
        double distFromOneToTwo = oneToTwo.magnitude();
        double sumRadii = One.getRadius() + Two.getRadius();
        double distBetween = distFromOneToTwo - sumRadii;
        
        double distBetweenSq = distBetween * distBetween;
        
        //if the entities aren't moving fast enough (assuming they are traveling in a straight line) 
        //to reach the other, they can't possibly be colliding
        if(relativeVelocity.magnitudeSq() < distBetweenSq)
        {
            return false;
        }

        
        //Make sure that One is moving twoards Two, if not return early
        double velocityDotOneTwoTwo = relativeVelocity.dot(oneToTwo);
        
        if(velocityDotOneTwoTwo <= 0)
        {
            return false;
        }
        
        
        //calculate the distance between One and the closest it could 
        //ever be to two by projecting the vector from one to two onto the normalized
        //velocity. 
        Vector2D normalRelativeVelocity = relativeVelocity.getNormalized();
        double distFromOneToClosestOneCouldBeToTwo = normalRelativeVelocity.dot(oneToTwo);
      
        double distFromClosestOneCouldBeToTwoToTwoSq = (distFromOneToTwo * distFromOneToTwo) - 
                (distFromOneToClosestOneCouldBeToTwo * distFromOneToClosestOneCouldBeToTwo);
        
        double sumRadiiSq = sumRadii * sumRadii;
        
        //if the closest one could be to two is further away from two 
        //than the sum of the radii there will be a flyby
        if(distFromClosestOneCouldBeToTwoToTwoSq >= sumRadiiSq)
        {
            return false;
        }

        //At this point we know there is a collision immenant, we just have to see
        //if it will occur within delta seconds or shortly thereafter. 
        //We look at the closest the two entities will ever be and from that point.
        //We calculate how far away from that point the entities would first collide
        //and then check if the entity will move at least as far as is required 
        //to actually collide in the itme frame
        double distFromInitialCollisionToClosestOneCouldBeToTwo = Math.sqrt(sumRadiiSq - distFromClosestOneCouldBeToTwoToTwoSq);
        
        double distNeededForCollision = distFromOneToClosestOneCouldBeToTwo - distFromInitialCollisionToClosestOneCouldBeToTwo;
        double distNeededForCollisionSq = distNeededForCollision * distNeededForCollision;
        
        if(relativeVelocity.magnitudeSq() < distNeededForCollisionSq)
        {
            return false;
        }
        
        //At this point we know there has to be a collision happening at some point during 
        //delta
        return true;
    }

  
    /**
     * Respond to a collision between two entities. This assumes two entities 
     * are colliding. The entities will be separated so they are just touching
     * and their velocities will be adjusted according to real world conservation 
     * of momentum.
     * 
     * @param one the first entity 
     * @param two the second entity 
     * 
     */
    public synchronized void collisionResponse(AbstractEntity one, AbstractEntity two)
    {
        //calculate how much to seperate the entities by along the collision vector
        
        double sumRadii = one.getRadius() + two.getRadius();
        
        //the collision vector from one to two (head minus tail)
        Vector2D collisionNormal = two.getLocation().minus(one.getLocation());
        double dist = collisionNormal.magnitude();
        collisionNormal.normalize();
        
        double penetration = sumRadii - dist;
        
        //each entity will be pushed out by half the total penetration aong the collision vector
        Vector2D responseVec = collisionNormal.times(penetration * .5);
        
        //the collision was from one to two so move one backwards
        one.getLocation().plusEquals(responseVec.times(-1.0));
        two.getLocation().plusEquals(responseVec);
        
        //calculate the collision response for the entities (see conservation of momentum for more info)
        
        Vector2D relativeVelocity = one.getVelocity().minus(two.getVelocity());
        
        //compute the impulse factor 
        double vDotN = relativeVelocity.dot(collisionNormal);
        double numerator = -(1.0 + elasticity) * vDotN;
        double denominator = ((1.0/one.getMass()) + (1.0/two.getMass()));
        double impulseFactor = numerator/denominator;
        
        //adjust velocities (heading and speed) based on the impulse factor, mass, and the collision ector
        Vector2D oneNewVelocity = one.getVelocity().plus(collisionNormal.times(impulseFactor/one.getMass()));
        double oneSpeed = oneNewVelocity.magnitude();
        one.setHeading(oneNewVelocity.getNormalized());
        one.setSpeed(oneSpeed);
        
        Vector2D twoNewVelocity = two.getVelocity().minus(collisionNormal.times(impulseFactor/two.getMass()));
        double twoSpeed = twoNewVelocity.magnitude();
        two.setHeading(twoNewVelocity.getNormalized());
        two.setSpeed(twoSpeed);
        
    }

}







/********************************************************
 *
 *  THIS IS EXPERIMENTAL CODE 
 * 
 ********************************************************/

//Expermental code for general wall collision instead of collision with world bounds 
    
//    /**
//     * Check if an entity will collide with the bounds of a rectangular 
//     * world during the specified duration of time (in seconds)
//     * 
//     * @param entity the entity to check for collisions with the world
//     * @param world to check for collisions within (the bounds of the world)
//     * @param delta the duration in which to check for a collision in seconds 
//     * 
//     * @return true if the entity will collide with the bounds of world
//     */
//    public synchronized boolean isCollsion(AbstractEntity entity, WallEntity wall, double delta)
//    {
//        
//        Point2D startLoc = new Point2D(entity.getLocation());
//
//        
//        
//        
//        if(wall.isHorizontal())
//        {
//            if(wall.getEnd().getX())
//        }
//        Point2D entityStart = new Point2D(entity.getLocation());
//        
//        
//        Vector2D movementVec = entity.getHeading().times(delta * entity.getSpeed());
//        
//        Vector2D radiusVectorInHeadingDirection = entity.getHeading().times(entity.getRadius());
//        
//        movementVec.plusEquals(radiusVectorInHeadingDirection);
//        
//        Point2D entityEnd = new Point2D(entityStart.plus(movementVec));
//
//        
//         
//        
//        
//        Point2D wallStart = wall.getStart();
//        Point2D wallEnd = wall.getEnd();
//        
//        
//        boolean intersects = Line2D.linesIntersect(entityStart.getX(), entityStart.getY(), entityEnd.getX(), entityEnd.getY(),
//                 wallStart.getX(), wallStart.getY(), wallEnd.getX(), wallEnd.getY());
//        
//      
//        return intersects;
//        
//    }
//    
//    /**
//     * Respond to a collision between an entity and the world bounds
//     * 
//     * @param entity
//     * @param world
//     * @param delta 
//     */
//    public synchronized void collisionResponse(AbstractEntity entity, WallEntity wall, double delta)
//    {
//       if(wall.isHorizontal())
//       {
//           entity.getHeading().reflectY();
//       }
//       else if(wall.isVertical())
//       {
//           entity.getHeading().reflectX();
//       }
//       else
//       {
//           throw new IllegalStateException("Only horizontal and vertical walls are currently supported by collision response for walls");
//       }
//       
//        
//        
//    } 
//    